#include "camodocal/chessboard/Chessboard.h"

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "camodocal/chessboard/ChessboardQuad.h"
#include "camodocal/chessboard/Spline.h"

#define MAX_CONTOUR_APPROX 7

namespace camodocal
{

    Chessboard::Chessboard(cv::Size boardSize, cv::Mat &image)
        : mBoardSize(boardSize), mCornersFound(false)
    {
        if (image.channels() == 1)
        {
            cv::cvtColor(image, mSketch, CV_GRAY2BGR);
            image.copyTo(mImage);
        }
        else
        {
            image.copyTo(mSketch);
            cv::cvtColor(image, mImage, CV_BGR2GRAY);
        }
    }

    void
    Chessboard::findCorners(bool useOpenCV)
    {
        mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners,
                                              CV_CALIB_CB_ADAPTIVE_THRESH +
                                                  CV_CALIB_CB_NORMALIZE_IMAGE +
                                                  CV_CALIB_CB_FILTER_QUADS +
                                                  CV_CALIB_CB_FAST_CHECK,
                                              useOpenCV);

        if (mCornersFound)
        {
            // draw chessboard corners
            cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound);
        }
    }

    const std::vector<cv::Point2f> &
    Chessboard::getCorners(void) const
    {
        return mCorners;
    }

    bool
    Chessboard::cornersFound(void) const
    {
        return mCornersFound;
    }

    const cv::Mat &
    Chessboard::getImage(void) const
    {
        return mImage;
    }

    const cv::Mat &
    Chessboard::getSketch(void) const
    {
        return mSketch;
    }

    bool
    Chessboard::findChessboardCorners(const cv::Mat &image,
                                      const cv::Size &patternSize,
                                      std::vector<cv::Point2f> &corners,
                                      int flags, bool useOpenCV)
    {
        if (useOpenCV)
        {
            return cv::findChessboardCorners(image, patternSize, corners, flags);
        }
        else
        {
            return findChessboardCornersImproved(image, patternSize, corners, flags);
        }
    }

    bool
    Chessboard::findChessboardCornersImproved(const cv::Mat &image,
                                              const cv::Size &patternSize,
                                              std::vector<cv::Point2f> &corners,
                                              int flags)
    {
        /************************************************************************************\
            This is improved variant of chessboard corner detection algorithm that
            uses a graph of connected quads. It is based on the code contributed
            by Vladimir Vezhnevets and Philip Gruebele.
            Here is the copyright notice from the original Vladimir's code:
            ===============================================================

            The algorithms developed and implemented by Vezhnevets Vldimir
            aka Dead Moroz (vvp@graphics.cs.msu.ru)
            See http://graphics.cs.msu.su/en/research/calibration/opencv.html
            for detailed information.

            Reliability additions and modifications made by Philip Gruebele.
            <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>

            Some improvements were made by:
            1) Martin Rufli - increased chance of correct corner matching
               Rufli, M., Scaramuzza, D., and Siegwart, R. (2008),
               Automatic Detection of Checkerboards on Blurred and Distorted Images,
               Proceedings of the IEEE/RSJ International Conference on
               Intelligent Robots and Systems (IROS 2008), Nice, France, September 2008.
            2) Lionel Heng - post-detection checks and better thresholding

        \************************************************************************************/

        // int bestDilation        = -1;
        const int minDilations = 0;
        const int maxDilations = 7;

        std::vector<ChessboardQuadPtr> outputQuadGroup;

        if (image.depth() != CV_8U || image.channels() == 2)
        {
            return false;
        }

        if (patternSize.width < 2 || patternSize.height < 2)
        {
            return false;
        }

        if (patternSize.width > 127 || patternSize.height > 127)
        {
            return false;
        }

        cv::Mat img = image;

        // Image histogram normalization and
        // BGR to Grayscale image conversion (if applicable)
        // MARTIN: Set to "false"
        if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE))
        {
            cv::Mat norm_img(image.rows, image.cols, CV_8UC1);

            if (image.channels() != 1)
            {
                cv::cvtColor(image, norm_img, CV_BGR2GRAY);
                img = norm_img;
            }

            if (flags & CV_CALIB_CB_NORMALIZE_IMAGE)
            {
                cv::equalizeHist(image, norm_img);
                img = norm_img;
            }
        }

        if (flags & CV_CALIB_CB_FAST_CHECK)
        {
            if (!checkChessboard(img, patternSize))
            {
                return false;
            }
        }

        // PART 1: FIND LARGEST PATTERN
        //-----------------------------------------------------------------------
        // Checker patterns are tried to be found by dilating the background and
        // then applying a canny edge finder on the closed contours (checkers).
        // Try one dilation run, but if the pattern is not found, repeat until
        // max_dilations is reached.

        int prevSqrSize = 0;
        bool found = false;
        std::vector<ChessboardCornerPtr> outputCorners;

        for (int k = 0; k < 6; ++k)
        {
            for (int dilations = minDilations; dilations <= maxDilations; ++dilations)
            {
                if (found)
                {
                    break;
                }

                cv::Mat thresh_img;

                // convert the input grayscale image to binary (black-n-white)
                if (flags & CV_CALIB_CB_ADAPTIVE_THRESH)
                {
                    int blockSize = lround(prevSqrSize == 0 ? std::min(img.cols, img.rows) * (k % 2 == 0 ? 0.2 : 0.1) : prevSqrSize * 2) | 1;

                    // convert to binary
                    cv::adaptiveThreshold(img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, blockSize, (k / 2) * 5);
                }
                else
                {
                    // empiric threshold level
                    double mean = (cv::mean(img))[0];
                    int thresh_level = lround(mean - 10);
                    thresh_level = std::max(thresh_level, 10);

                    cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY);
                }

                // MARTIN's Code
                // Use both a rectangular and a cross kernel. In this way, a more
                // homogeneous dilation is performed, which is crucial for small,
                // distorted checkers. Use the CROSS kernel first, since its action
                // on the image is more subtle
                cv::Mat kernel1 = cv::getStructuringElement(CV_SHAPE_CROSS, cv::Size(3, 3), cv::Point(1, 1));
                cv::Mat kernel2 = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(3, 3), cv::Point(1, 1));

                if (dilations >= 1)
                    cv::dilate(thresh_img, thresh_img, kernel1);
                if (dilations >= 2)
                    cv::dilate(thresh_img, thresh_img, kernel2);
                if (dilations >= 3)
                    cv::dilate(thresh_img, thresh_img, kernel1);
                if (dilations >= 4)
                    cv::dilate(thresh_img, thresh_img, kernel2);
                if (dilations >= 5)
                    cv::dilate(thresh_img, thresh_img, kernel1);
                if (dilations >= 6)
                    cv::dilate(thresh_img, thresh_img, kernel2);

                // In order to find rectangles that go to the edge, we draw a white
                // line around the image edge. Otherwise FindContours will miss those
                // clipped rectangle contours. The border color will be the image mean,
                // because otherwise we risk screwing up filters like cvSmooth()
                cv::rectangle(thresh_img, cv::Point(0, 0),
                              cv::Point(thresh_img.cols - 1, thresh_img.rows - 1),
                              CV_RGB(255, 255, 255), 3, 8);

                // Generate quadrangles in the following function
                std::vector<ChessboardQuadPtr> quads;

                generateQuads(quads, thresh_img, flags, dilations, true);
                if (quads.empty())
                {
                    continue;
                }

                // The following function finds and assigns neighbor quads to every
                // quadrangle in the immediate vicinity fulfilling certain
                // prerequisites
                findQuadNeighbors(quads, dilations);

                // The connected quads will be organized in groups. The following loop
                // increases a "group_idx" identifier.
                // The function "findConnectedQuads assigns all connected quads
                // a unique group ID.
                // If more quadrangles were assigned to a given group (i.e. connected)
                // than are expected by the input variable "patternSize", the
                // function "cleanFoundConnectedQuads" erases the surplus
                // quadrangles by minimizing the convex hull of the remaining pattern.

                for (int group_idx = 0;; ++group_idx)
                {
                    std::vector<ChessboardQuadPtr> quadGroup;

                    findConnectedQuads(quads, quadGroup, group_idx, dilations);

                    if (quadGroup.empty())
                    {
                        break;
                    }

                    cleanFoundConnectedQuads(quadGroup, patternSize);

                    // The following function labels all corners of every quad
                    // with a row and column entry.
                    // "count" specifies the number of found quads in "quad_group"
                    // with group identifier "group_idx"
                    // The last parameter is set to "true", because this is the
                    // first function call and some initializations need to be
                    // made.
                    labelQuadGroup(quadGroup, patternSize, true);

                    found = checkQuadGroup(quadGroup, outputCorners, patternSize);

                    float sumDist = 0;
                    int total = 0;

                    for (int i = 0; i < (int)outputCorners.size(); ++i)
                    {
                        int ni = 0;
                        float avgi = outputCorners.at(i)->meanDist(ni);
                        sumDist += avgi * ni;
                        total += ni;
                    }
                    prevSqrSize = lround(sumDist / std::max(total, 1));

                    if (found && !checkBoardMonotony(outputCorners, patternSize))
                    {
                        found = false;
                    }
                }
            }
        }

        if (!found)
        {
            return false;
        }
        else
        {
            corners.clear();
            corners.reserve(outputCorners.size());
            for (size_t i = 0; i < outputCorners.size(); ++i)
            {
                corners.push_back(outputCorners.at(i)->pt);
            }

            cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1, -1),
                             cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

            return true;
        }
    }

    //===========================================================================
    // ERASE OVERHEAD
    //===========================================================================
    // If we found too many connected quads, remove those which probably do not
    // belong.
    void
    Chessboard::cleanFoundConnectedQuads(std::vector<ChessboardQuadPtr> &quadGroup,
                                         cv::Size patternSize)
    {
        cv::Point2f center(0.0f, 0.0f);

        // Number of quads this pattern should contain
        int count = ((patternSize.width + 1) * (patternSize.height + 1) + 1) / 2;

        // If we have more quadrangles than we should, try to eliminate duplicates
        // or ones which don't belong to the pattern rectangle. Else go to the end
        // of the function
        if ((int)quadGroup.size() <= count)
        {
            return;
        }

        // Create an array of quadrangle centers
        std::vector<cv::Point2f> centers;
        centers.resize(quadGroup.size());

        for (size_t i = 0; i < quadGroup.size(); ++i)
        {
            cv::Point2f ci(0.0f, 0.0f);
            ChessboardQuadPtr &q = quadGroup[i];

            for (int j = 0; j < 4; ++j)
            {
                ci += q->corners[j]->pt;
            }

            ci *= 0.25f;

            // Centers(i), is the geometric center of quad(i)
            // Center, is the center of all found quads
            centers[i] = ci;

            center += ci;
        }

        center *= 1.0f / quadGroup.size();

        // If we have more quadrangles than we should, we try to eliminate bad
        // ones based on minimizing the bounding box. We iteratively remove the
        // point which reduces the size of the bounding box of the blobs the most
        // (since we want the rectangle to be as small as possible) remove the
        // quadrange that causes the biggest reduction in pattern size until we
        // have the correct number
        while ((int)quadGroup.size() > count)
        {
            double minBoxArea = DBL_MAX;
            int minBoxAreaIndex = -1;

            // For each point, calculate box area without that point
            for (size_t skip = 0; skip < quadGroup.size(); ++skip)
            {
                // get bounding rectangle
                cv::Point2f temp = centers[skip];
                centers[skip] = center;

                std::vector<cv::Point2f> hull;
                cv::convexHull(centers, hull, true, true);
                centers[skip] = temp;

                double hull_area = fabs(cv::contourArea(hull));

                // remember smallest box area
                if (hull_area < minBoxArea)
                {
                    minBoxArea = hull_area;
                    minBoxAreaIndex = skip;
                }
            }

            ChessboardQuadPtr &q0 = quadGroup[minBoxAreaIndex];

            // remove any references to this quad as a neighbor
            for (size_t i = 0; i < quadGroup.size(); ++i)
            {
                ChessboardQuadPtr &q = quadGroup.at(i);
                for (int j = 0; j < 4; ++j)
                {
                    if (q->neighbors[j] == q0)
                    {
                        q->neighbors[j].reset();
                        q->count--;
                        for (int k = 0; k < 4; ++k)
                        {
                            if (q0->neighbors[k] == q)
                            {
                                q0->neighbors[k].reset();
                                q0->count--;
                                break;
                            }
                        }
                        break;
                    }
                }
            }
            // remove the quad
            quadGroup.at(minBoxAreaIndex) = quadGroup.back();
            centers.at(minBoxAreaIndex) = centers.back();
            quadGroup.pop_back();
            centers.pop_back();
        }
    }

    //===========================================================================
    // FIND COONECTED QUADS
    //===========================================================================
    void
    Chessboard::findConnectedQuads(std::vector<ChessboardQuadPtr> &quads,
                                   std::vector<ChessboardQuadPtr> &group,
                                   int group_idx, int dilation)
    {
        ChessboardQuadPtr q;

        // Scan the array for a first unlabeled quad
        for (size_t i = 0; i < quads.size(); ++i)
        {
            ChessboardQuadPtr &quad = quads.at(i);

            if (quad->count > 0 && quad->group_idx < 0)
            {
                q = quad;
                break;
            }
        }

        if (q.get() == 0)
        {
            return;
        }

        // Recursively find a group of connected quads starting from the seed quad

        std::vector<ChessboardQuadPtr> stack;
        stack.push_back(q);

        group.push_back(q);
        q->group_idx = group_idx;

        while (!stack.empty())
        {
            q = stack.back();
            stack.pop_back();

            for (int i = 0; i < 4; ++i)
            {
                ChessboardQuadPtr &neighbor = q->neighbors[i];

                // If he neighbor exists and the neighbor has more than 0
                // neighbors and the neighbor has not been classified yet.
                if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0)
                {
                    stack.push_back(neighbor);
                    group.push_back(neighbor);
                    neighbor->group_idx = group_idx;
                }
            }
        }
    }

    void
    Chessboard::labelQuadGroup(std::vector<ChessboardQuadPtr> &quadGroup,
                               cv::Size patternSize, bool firstRun)
    {
        // If this is the first function call, a seed quad needs to be selected
        if (firstRun)
        {
            // Search for the (first) quad with the maximum number of neighbors
            // (usually 4). This will be our starting point.
            int mark = -1;
            int maxNeighborCount = 0;
            for (size_t i = 0; i < quadGroup.size(); ++i)
            {
                ChessboardQuadPtr &q = quadGroup.at(i);
                if (q->count > maxNeighborCount)
                {
                    mark = i;
                    maxNeighborCount = q->count;

                    if (maxNeighborCount == 4)
                    {
                        break;
                    }
                }
            }

            // Mark the starting quad's (per definition) upper left corner with
            //(0,0) and then proceed clockwise
            // The following labeling sequence enshures a "right coordinate system"
            ChessboardQuadPtr &q = quadGroup.at(mark);

            q->labeled = true;

            q->corners[0]->row = 0;
            q->corners[0]->column = 0;
            q->corners[1]->row = 0;
            q->corners[1]->column = 1;
            q->corners[2]->row = 1;
            q->corners[2]->column = 1;
            q->corners[3]->row = 1;
            q->corners[3]->column = 0;
        }

        // Mark all other corners with their respective row and column
        bool flagChanged = true;
        while (flagChanged)
        {
            // First reset the flag to "false"
            flagChanged = false;

            // Go through all quads top down is faster, since unlabeled quads will
            // be inserted at the end of the list
            for (int i = quadGroup.size() - 1; i >= 0; --i)
            {
                ChessboardQuadPtr &quad = quadGroup.at(i);

                // Check whether quad "i" has been labeled already
                if (!quad->labeled)
                {
                    // Check its neighbors, whether some of them have been labeled
                    // already
                    for (int j = 0; j < 4; j++)
                    {
                        // Check whether the neighbor exists (i.e. is not the NULL
                        // pointer)
                        if (quad->neighbors[j])
                        {
                            ChessboardQuadPtr &quadNeighbor = quad->neighbors[j];

                            // Only proceed, if neighbor "j" was labeled
                            if (quadNeighbor->labeled)
                            {
                                // For every quad it could happen to pass here
                                // multiple times. We therefore "break" later.
                                // Check whitch of the neighbors corners is
                                // connected to the current quad
                                int connectedNeighborCornerId = -1;
                                for (int k = 0; k < 4; ++k)
                                {
                                    if (quadNeighbor->neighbors[k] == quad)
                                    {
                                        connectedNeighborCornerId = k;

                                        // there is only one, therefore
                                        break;
                                    }
                                }

                                // For the following calculations we need the row
                                // and column of the connected neighbor corner and
                                // all other corners of the connected quad "j",
                                // clockwise (CW)
                                ChessboardCornerPtr &conCorner = quadNeighbor->corners[connectedNeighborCornerId];
                                ChessboardCornerPtr &conCornerCW1 = quadNeighbor->corners[(connectedNeighborCornerId + 1) % 4];
                                ChessboardCornerPtr &conCornerCW2 = quadNeighbor->corners[(connectedNeighborCornerId + 2) % 4];
                                ChessboardCornerPtr &conCornerCW3 = quadNeighbor->corners[(connectedNeighborCornerId + 3) % 4];

                                quad->corners[j]->row = conCorner->row;
                                quad->corners[j]->column = conCorner->column;
                                quad->corners[(j + 1) % 4]->row = conCorner->row - conCornerCW2->row + conCornerCW3->row;
                                quad->corners[(j + 1) % 4]->column = conCorner->column - conCornerCW2->column + conCornerCW3->column;
                                quad->corners[(j + 2) % 4]->row = conCorner->row + conCorner->row - conCornerCW2->row;
                                quad->corners[(j + 2) % 4]->column = conCorner->column + conCorner->column - conCornerCW2->column;
                                quad->corners[(j + 3) % 4]->row = conCorner->row - conCornerCW2->row + conCornerCW1->row;
                                quad->corners[(j + 3) % 4]->column = conCorner->column - conCornerCW2->column + conCornerCW1->column;

                                // Mark this quad as labeled
                                quad->labeled = true;

                                // Changes have taken place, set the flag
                                flagChanged = true;

                                // once is enough!
                                break;
                            }
                        }
                    }
                }
            }
        }

        // All corners are marked with row and column
        // Record the minimal and maximal row and column indices
        // It is unlikely that more than 8bit checkers are used per dimension, if there are
        // an error would have been thrown at the beginning of "cvFindChessboardCorners2"
        int min_row = 127;
        int max_row = -127;
        int min_column = 127;
        int max_column = -127;

        for (int i = 0; i < (int)quadGroup.size(); ++i)
        {
            ChessboardQuadPtr &q = quadGroup.at(i);

            for (int j = 0; j < 4; ++j)
            {
                ChessboardCornerPtr &c = q->corners[j];

                if (c->row > max_row)
                {
                    max_row = c->row;
                }
                if (c->row < min_row)
                {
                    min_row = c->row;
                }
                if (c->column > max_column)
                {
                    max_column = c->column;
                }
                if (c->column < min_column)
                {
                    min_column = c->column;
                }
            }
        }

        // Label all internal corners with "needsNeighbor" = false
        // Label all external corners with "needsNeighbor" = true,
        // except if in a given dimension the pattern size is reached
        for (int i = min_row; i <= max_row; ++i)
        {
            for (int j = min_column; j <= max_column; ++j)
            {
                // A flag that indicates, whether a row/column combination is
                // executed multiple times
                bool flag = false;

                // Remember corner and quad
                int cornerID;
                int quadID;

                for (int k = 0; k < (int)quadGroup.size(); ++k)
                {
                    ChessboardQuadPtr &q = quadGroup.at(k);

                    for (int l = 0; l < 4; ++l)
                    {
                        if ((q->corners[l]->row == i) && (q->corners[l]->column == j))
                        {
                            if (flag)
                            {
                                // Passed at least twice through here
                                q->corners[l]->needsNeighbor = false;
                                quadGroup[quadID]->corners[cornerID]->needsNeighbor = false;
                            }
                            else
                            {
                                // Mark with needs a neighbor, but note the
                                // address
                                q->corners[l]->needsNeighbor = true;
                                cornerID = l;
                                quadID = k;
                            }

                            // set the flag to true
                            flag = true;
                        }
                    }
                }
            }
        }

        // Complete Linking:
        // sometimes not all corners were properly linked in "findQuadNeighbors",
        // but after labeling each corner with its respective row and column, it is
        // possible to match them anyway.
        for (int i = min_row; i <= max_row; ++i)
        {
            for (int j = min_column; j <= max_column; ++j)
            {
                // the following "number" indicates the number of corners which
                // correspond to the given (i,j) value
                // 1    is a border corner or a conrer which still needs a neighbor
                // 2    is a fully connected internal corner
                // >2    something went wrong during labeling, report a warning
                int number = 1;

                // remember corner and quad
                int cornerID;
                int quadID;

                for (int k = 0; k < (int)quadGroup.size(); ++k)
                {
                    ChessboardQuadPtr &q = quadGroup.at(k);

                    for (int l = 0; l < 4; ++l)
                    {
                        if ((q->corners[l]->row == i) && (q->corners[l]->column == j))
                        {

                            if (number == 1)
                            {
                                // First corner, note its ID
                                cornerID = l;
                                quadID = k;
                            }

                            else if (number == 2)
                            {
                                // Second corner, check wheter this and the
                                // first one have equal coordinates, else
                                // interpolate
                                cv::Point2f delta = q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt;

                                if (delta.x != 0.0f || delta.y != 0.0f)
                                {
                                    // Interpolate
                                    q->corners[l]->pt -= delta * 0.5f;

                                    quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f;
                                }
                            }
                            else if (number > 2)
                            {
                                // Something went wrong during row/column labeling
                                // Report a Warning
                                // ->Implemented in the function "mrWriteCorners"
                            }

                            // increase the number by one
                            ++number;
                        }
                    }
                }
            }
        }

        // Bordercorners don't need any neighbors, if the pattern size in the
        // respective direction is reached
        // The only time we can make shure that the target pattern size is reached in a given
        // dimension, is when the larger side has reached the target size in the maximal
        // direction, or if the larger side is larger than the smaller target size and the
        // smaller side equals the smaller target size
        int largerDimPattern = std::max(patternSize.height, patternSize.width);
        int smallerDimPattern = std::min(patternSize.height, patternSize.width);
        bool flagSmallerDim1 = false;
        bool flagSmallerDim2 = false;

        if ((largerDimPattern + 1) == max_column - min_column)
        {
            flagSmallerDim1 = true;
            // We found out that in the column direction the target pattern size is reached
            // Therefore border column corners do not need a neighbor anymore
            // Go through all corners
            for (int k = 0; k < (int)quadGroup.size(); ++k)
            {
                ChessboardQuadPtr &q = quadGroup.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    ChessboardCornerPtr &c = q->corners[l];

                    if (c->column == min_column || c->column == max_column)
                    {
                        // Needs no neighbor anymore
                        c->needsNeighbor = false;
                    }
                }
            }
        }

        if ((largerDimPattern + 1) == max_row - min_row)
        {
            flagSmallerDim2 = true;
            // We found out that in the column direction the target pattern size is reached
            // Therefore border column corners do not need a neighbor anymore
            // Go through all corners
            for (int k = 0; k < (int)quadGroup.size(); ++k)
            {
                ChessboardQuadPtr &q = quadGroup.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    ChessboardCornerPtr &c = q->corners[l];

                    if (c->row == min_row || c->row == max_row)
                    {
                        // Needs no neighbor anymore
                        c->needsNeighbor = false;
                    }
                }
            }
        }

        // Check the two flags:
        //    -    If one is true and the other false, then the pattern target
        //        size was reached in in one direction -> We can check, whether the target
        //        pattern size is also reached in the other direction
        //  -    If both are set to true, then we deal with a square board -> do nothing
        //  -    If both are set to false -> There is a possibility that the larger side is
        //        larger than the smaller target size -> Check and if true, then check whether
        //        the other side has the same size as the smaller target size
        if ((flagSmallerDim1 == false && flagSmallerDim2 == true))
        {
            // Larger target pattern size is in row direction, check wheter smaller target
            // pattern size is reached in column direction
            if ((smallerDimPattern + 1) == max_column - min_column)
            {
                for (int k = 0; k < (int)quadGroup.size(); ++k)
                {
                    ChessboardQuadPtr &q = quadGroup.at(k);

                    for (int l = 0; l < 4; ++l)
                    {
                        ChessboardCornerPtr &c = q->corners[l];

                        if (c->column == min_column || c->column == max_column)
                        {
                            // Needs no neighbor anymore
                            c->needsNeighbor = false;
                        }
                    }
                }
            }
        }

        if ((flagSmallerDim1 == true && flagSmallerDim2 == false))
        {
            // Larger target pattern size is in column direction, check wheter smaller target
            // pattern size is reached in row direction
            if ((smallerDimPattern + 1) == max_row - min_row)
            {
                for (int k = 0; k < (int)quadGroup.size(); ++k)
                {
                    ChessboardQuadPtr &q = quadGroup.at(k);

                    for (int l = 0; l < 4; ++l)
                    {
                        ChessboardCornerPtr &c = q->corners[l];

                        if (c->row == min_row || c->row == max_row)
                        {
                            // Needs no neighbor anymore
                            c->needsNeighbor = false;
                        }
                    }
                }
            }
        }

        if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_column - min_column)
        {
            // Larger target pattern size is in column direction, check wheter smaller target
            // pattern size is reached in row direction
            if ((smallerDimPattern + 1) == max_row - min_row)
            {
                for (int k = 0; k < (int)quadGroup.size(); ++k)
                {
                    ChessboardQuadPtr &q = quadGroup.at(k);

                    for (int l = 0; l < 4; ++l)
                    {
                        ChessboardCornerPtr &c = q->corners[l];

                        if (c->row == min_row || c->row == max_row)
                        {
                            // Needs no neighbor anymore
                            c->needsNeighbor = false;
                        }
                    }
                }
            }
        }

        if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_row - min_row)
        {
            // Larger target pattern size is in row direction, check wheter smaller target
            // pattern size is reached in column direction
            if ((smallerDimPattern + 1) == max_column - min_column)
            {
                for (int k = 0; k < (int)quadGroup.size(); ++k)
                {
                    ChessboardQuadPtr &q = quadGroup.at(k);

                    for (int l = 0; l < 4; ++l)
                    {
                        ChessboardCornerPtr &c = q->corners[l];

                        if (c->column == min_column || c->column == max_column)
                        {
                            // Needs no neighbor anymore
                            c->needsNeighbor = false;
                        }
                    }
                }
            }
        }
    }

    //===========================================================================
    // GIVE A GROUP IDX
    //===========================================================================
    void
    Chessboard::findQuadNeighbors(std::vector<ChessboardQuadPtr> &quads, int dilation)
    {
        // Thresh dilation is used to counter the effect of dilation on the
        // distance between 2 neighboring corners. Since the distance below is
        // computed as its square, we do here the same. Additionally, we take the
        // conservative assumption that dilation was performed using the 3x3 CROSS
        // kernel, which coresponds to the 4-neighborhood.
        const float thresh_dilation = (float)(2 * dilation + 3) * (2 * dilation + 3) * 2; // the "*2" is for the x and y component
                                                                                          // the "3" is for initial corner mismatch

        // Find quad neighbors
        for (size_t idx = 0; idx < quads.size(); ++idx)
        {
            ChessboardQuadPtr &curQuad = quads.at(idx);

            // Go through all quadrangles and label them in groups
            // For each corner of this quadrangle
            for (int i = 0; i < 4; ++i)
            {
                float minDist = FLT_MAX;
                int closestCornerIdx = -1;
                ChessboardQuadPtr closestQuad;

                if (curQuad->neighbors[i])
                {
                    continue;
                }

                cv::Point2f pt = curQuad->corners[i]->pt;

                // Find the closest corner in all other quadrangles
                for (size_t k = 0; k < quads.size(); ++k)
                {
                    if (k == idx)
                    {
                        continue;
                    }

                    ChessboardQuadPtr &quad = quads.at(k);

                    for (int j = 0; j < 4; ++j)
                    {
                        // If it already has a neighbor
                        if (quad->neighbors[j])
                        {
                            continue;
                        }

                        cv::Point2f dp = pt - quad->corners[j]->pt;
                        float dist = dp.dot(dp);

                        // The following "if" checks, whether "dist" is the
                        // shortest so far and smaller than the smallest
                        // edge length of the current and target quads
                        if (dist < minDist &&
                            dist <= (curQuad->edge_len + thresh_dilation) &&
                            dist <= (quad->edge_len + thresh_dilation))
                        {
                            // Check whether conditions are fulfilled
                            if (matchCorners(curQuad, i, quad, j))
                            {
                                closestCornerIdx = j;
                                closestQuad = quad;
                                minDist = dist;
                            }
                        }
                    }
                }

                // Have we found a matching corner point?
                if (closestCornerIdx >= 0 && minDist < FLT_MAX)
                {
                    ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];

                    // Make sure that the closest quad does not have the current
                    // quad as neighbor already
                    bool valid = true;
                    for (int j = 0; j < 4; ++j)
                    {
                        if (closestQuad->neighbors[j] == curQuad)
                        {
                            valid = false;
                            break;
                        }
                    }
                    if (!valid)
                    {
                        continue;
                    }

                    // We've found one more corner - remember it
                    closestCorner->pt = (pt + closestCorner->pt) * 0.5f;

                    curQuad->count++;
                    curQuad->neighbors[i] = closestQuad;
                    curQuad->corners[i] = closestCorner;

                    closestQuad->count++;
                    closestQuad->neighbors[closestCornerIdx] = curQuad;
                    closestQuad->corners[closestCornerIdx] = closestCorner;
                }
            }
        }
    }

    //===========================================================================
    // AUGMENT PATTERN WITH ADDITIONAL QUADS
    //===========================================================================
    // The first part of the function is basically a copy of
    // "findQuadNeighbors"
    // The comparisons between two points and two lines could be computed in their
    // own function
    int
    Chessboard::augmentBestRun(std::vector<ChessboardQuadPtr> &candidateQuads, int candidateDilation,
                               std::vector<ChessboardQuadPtr> &existingQuads, int existingDilation)
    {
        // thresh dilation is used to counter the effect of dilation on the
        // distance between 2 neighboring corners. Since the distance below is
        // computed as its square, we do here the same. Additionally, we take the
        // conservative assumption that dilation was performed using the 3x3 CROSS
        // kernel, which coresponds to the 4-neighborhood.
        const float thresh_dilation = (2 * candidateDilation + 3) * (2 * existingDilation + 3) * 2; // the "*2" is for the x and y component

        // Search all old quads which have a neighbor that needs to be linked
        for (size_t idx = 0; idx < existingQuads.size(); ++idx)
        {
            ChessboardQuadPtr &curQuad = existingQuads.at(idx);

            // For each corner of this quadrangle
            for (int i = 0; i < 4; ++i)
            {
                float minDist = FLT_MAX;
                int closestCornerIdx = -1;
                ChessboardQuadPtr closestQuad;

                // If curQuad corner[i] is already linked, continue
                if (!curQuad->corners[i]->needsNeighbor)
                {
                    continue;
                }

                cv::Point2f pt = curQuad->corners[i]->pt;

                // Look for a match in all candidateQuads' corners
                for (size_t k = 0; k < candidateQuads.size(); ++k)
                {
                    ChessboardQuadPtr &candidateQuad = candidateQuads.at(k);

                    // Only look at unlabeled new quads
                    if (candidateQuad->labeled)
                    {
                        continue;
                    }

                    for (int j = 0; j < 4; ++j)
                    {
                        // Only proceed if they are less than dist away from each
                        // other
                        cv::Point2f dp = pt - candidateQuad->corners[j]->pt;
                        float dist = dp.dot(dp);

                        if ((dist < minDist) &&
                            dist <= (curQuad->edge_len + thresh_dilation) &&
                            dist <= (candidateQuad->edge_len + thresh_dilation))
                        {
                            if (matchCorners(curQuad, i, candidateQuad, j))
                            {
                                closestCornerIdx = j;
                                closestQuad = candidateQuad;
                                minDist = dist;
                            }
                        }
                    }
                }

                // Have we found a matching corner point?
                if (closestCornerIdx >= 0 && minDist < FLT_MAX)
                {
                    ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];
                    closestCorner->pt = (pt + closestCorner->pt) * 0.5f;

                    // We've found one more corner - remember it
                    // ATTENTION: write the corner x and y coordinates separately,
                    // else the crucial row/column entries will be overwritten !!!
                    curQuad->corners[i]->pt = closestCorner->pt;
                    curQuad->neighbors[i] = closestQuad;
                    closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt;

                    // Label closest quad as labeled. In this way we exclude it
                    // being considered again during the next loop iteration
                    closestQuad->labeled = true;

                    // We have a new member of the final pattern, copy it over
                    ChessboardQuadPtr newQuad(new ChessboardQuad);
                    newQuad->count = 1;
                    newQuad->edge_len = closestQuad->edge_len;
                    newQuad->group_idx = curQuad->group_idx; // the same as the current quad
                    newQuad->labeled = false;                // do it right afterwards

                    curQuad->neighbors[i] = newQuad;

                    // We only know one neighbor for sure
                    newQuad->neighbors[closestCornerIdx] = curQuad;

                    for (int j = 0; j < 4; j++)
                    {
                        newQuad->corners[j].reset(new ChessboardCorner);
                        newQuad->corners[j]->pt = closestQuad->corners[j]->pt;
                    }

                    existingQuads.push_back(newQuad);

                    // Start the function again
                    return -1;
                }
            }
        }

        // Finished, don't start the function again
        return 1;
    }

    //===========================================================================
    // GENERATE QUADRANGLES
    //===========================================================================
    void
    Chessboard::generateQuads(std::vector<ChessboardQuadPtr> &quads,
                              cv::Mat &image, int flags,
                              int dilation, bool firstRun)
    {
        // Empirical lower bound for the size of allowable quadrangles.
        // MARTIN, modified: Added "*0.1" in order to find smaller quads.
        int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1);

        std::vector<std::vector<cv::Point>> contours;
        std::vector<cv::Vec4i> hierarchy;

        // Initialize contour retrieving routine
        cv::findContours(image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

        std::vector<std::vector<cv::Point>> quadContours;

        for (size_t i = 0; i < contours.size(); ++i)
        {
            // Reject contours with a too small perimeter and contours which are
            // completely surrounded by another contour
            // MARTIN: If this function is called during PART 1, then the parameter "first run"
            // is set to "true". This guarantees, that only "nice" squares are detected.
            // During PART 2, we allow the polygonal approximation function below to
            // approximate more freely, which can result in recognized "squares" that are in
            // reality multiple blurred and sticked together squares.
            std::vector<cv::Point> &contour = contours.at(i);

            if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize)
            {
                continue;
            }

            int min_approx_level = 1, max_approx_level;
            if (firstRun)
            {
                max_approx_level = 3;
            }
            else
            {
                max_approx_level = MAX_CONTOUR_APPROX;
            }

            std::vector<cv::Point> approxContour;
            for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++)
            {
                cv::approxPolyDP(contour, approxContour, approx_level, true);

                if (approxContour.size() == 4)
                {
                    break;
                }
            }

            // Reject non-quadrangles
            if (approxContour.size() == 4 && cv::isContourConvex(approxContour))
            {
                double p = cv::arcLength(approxContour, true);
                double area = cv::contourArea(approxContour);

                cv::Point pt[4];
                for (int i = 0; i < 4; i++)
                {
                    pt[i] = approxContour[i];
                }

                cv::Point dp = pt[0] - pt[2];
                double d1 = sqrt(dp.dot(dp));

                dp = pt[1] - pt[3];
                double d2 = sqrt(dp.dot(dp));

                // PHILIPG: Only accept those quadrangles which are more
                // square than rectangular and which are big enough
                dp = pt[0] - pt[1];
                double d3 = sqrt(dp.dot(dp));
                dp = pt[1] - pt[2];
                double d4 = sqrt(dp.dot(dp));

                if (!(flags & CV_CALIB_CB_FILTER_QUADS) ||
                    (d3 * 4 > d4 && d4 * 4 > d3 && d3 * d4 < area * 1.5 && area > minSize &&
                     d1 >= 0.15 * p && d2 >= 0.15 * p))
                {
                    quadContours.push_back(approxContour);
                }
            }
        }

        // Allocate quad & corner buffers
        quads.resize(quadContours.size());

        // Create array of quads structures
        for (size_t idx = 0; idx < quadContours.size(); ++idx)
        {
            ChessboardQuadPtr &q = quads.at(idx);
            std::vector<cv::Point> &contour = quadContours.at(idx);

            // Reset group ID
            q.reset(new ChessboardQuad);
            assert(contour.size() == 4);

            for (int i = 0; i < 4; ++i)
            {
                cv::Point2f pt = contour.at(i);
                ChessboardCornerPtr corner(new ChessboardCorner);
                corner->pt = pt;
                q->corners[i] = corner;
            }

            for (int i = 0; i < 4; ++i)
            {
                cv::Point2f dp = q->corners[i]->pt - q->corners[(i + 1) & 3]->pt;
                float d = dp.dot(dp);
                if (q->edge_len > d)
                {
                    q->edge_len = d;
                }
            }
        }
    }

    bool
    Chessboard::checkQuadGroup(std::vector<ChessboardQuadPtr> &quads,
                               std::vector<ChessboardCornerPtr> &corners,
                               cv::Size patternSize)
    {
        // Initialize
        bool flagRow = false;
        bool flagColumn = false;
        int height = -1;
        int width = -1;

        // Compute the minimum and maximum row / column ID
        // (it is unlikely that more than 8bit checkers are used per dimension)
        int min_row = 127;
        int max_row = -127;
        int min_col = 127;
        int max_col = -127;

        for (size_t i = 0; i < quads.size(); ++i)
        {
            ChessboardQuadPtr &q = quads.at(i);

            for (int j = 0; j < 4; ++j)
            {
                ChessboardCornerPtr &c = q->corners[j];

                if (c->row > max_row)
                {
                    max_row = c->row;
                }
                if (c->row < min_row)
                {
                    min_row = c->row;
                }
                if (c->column > max_col)
                {
                    max_col = c->column;
                }
                if (c->column < min_col)
                {
                    min_col = c->column;
                }
            }
        }

        // If in a given direction the target pattern size is reached, we know exactly how
        // the checkerboard is oriented.
        // Else we need to prepare enough "dummy" corners for the worst case.
        for (size_t i = 0; i < quads.size(); ++i)
        {
            ChessboardQuadPtr &q = quads.at(i);

            for (int j = 0; j < 4; ++j)
            {
                ChessboardCornerPtr &c = q->corners[j];

                if (c->column == max_col && c->row != min_row && c->row != max_row && !c->needsNeighbor)
                {
                    flagColumn = true;
                }
                if (c->row == max_row && c->column != min_col && c->column != max_col && !c->needsNeighbor)
                {
                    flagRow = true;
                }
            }
        }

        if (flagColumn)
        {
            if (max_col - min_col == patternSize.width + 1)
            {
                width = patternSize.width;
                height = patternSize.height;
            }
            else
            {
                width = patternSize.height;
                height = patternSize.width;
            }
        }
        else if (flagRow)
        {
            if (max_row - min_row == patternSize.width + 1)
            {
                height = patternSize.width;
                width = patternSize.height;
            }
            else
            {
                height = patternSize.height;
                width = patternSize.width;
            }
        }
        else
        {
            // If target pattern size is not reached in at least one of the two
            // directions,  then we do not know where the remaining corners are
            // located. Account for this.
            width = std::max(patternSize.width, patternSize.height);
            height = std::max(patternSize.width, patternSize.height);
        }

        ++min_row;
        ++min_col;
        max_row = min_row + height - 1;
        max_col = min_col + width - 1;

        corners.clear();

        int linkedBorderCorners = 0;

        // Write the corners in increasing order to the output file
        for (int i = min_row; i <= max_row; ++i)
        {
            for (int j = min_col; j <= max_col; ++j)
            {
                // Reset the iterator
                int iter = 1;

                for (int k = 0; k < (int)quads.size(); ++k)
                {
                    ChessboardQuadPtr &quad = quads.at(k);

                    for (int l = 0; l < 4; ++l)
                    {
                        ChessboardCornerPtr &c = quad->corners[l];

                        if (c->row == i && c->column == j)
                        {
                            bool boardEdge = false;
                            if (i == min_row || i == max_row ||
                                j == min_col || j == max_col)
                            {
                                boardEdge = true;
                            }

                            if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge))
                            {
                                // The respective row and column have been found
                                corners.push_back(quad->corners[l]);
                            }

                            if (iter == 2 && boardEdge)
                            {
                                ++linkedBorderCorners;
                            }

                            // If the iterator is larger than two, this means that more than
                            // two corners have the same row / column entries. Then some
                            // linking errors must have occured and we should not use the found
                            // pattern
                            if (iter > 2)
                            {
                                return false;
                            }

                            iter++;
                        }
                    }
                }
            }
        }

        if ((int)corners.size() != patternSize.width * patternSize.height ||
            linkedBorderCorners < (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f)
        {
            return false;
        }

        // check that no corners lie at image boundary
        float border = 5.0f;
        for (int i = 0; i < (int)corners.size(); ++i)
        {
            ChessboardCornerPtr &c = corners.at(i);

            if (c->pt.x < border || c->pt.x > mImage.cols - border ||
                c->pt.y < border || c->pt.y > mImage.rows - border)
            {
                return false;
            }
        }

        // check if we need to transpose the board
        if (width != patternSize.width)
        {
            std::swap(width, height);

            std::vector<ChessboardCornerPtr> outputCorners;
            outputCorners.resize(corners.size());

            for (int i = 0; i < height; ++i)
            {
                for (int j = 0; j < width; ++j)
                {
                    outputCorners.at(i * width + j) = corners.at(j * height + i);
                }
            }

            corners = outputCorners;
        }

        // check if we need to revert the order in each row
        cv::Point2f p0 = corners.at(0)->pt;
        cv::Point2f p1 = corners.at(width - 1)->pt;
        cv::Point2f p2 = corners.at(width)->pt;

        if ((p1 - p0).cross(p2 - p0) < 0.0f)
        {
            for (int i = 0; i < height; ++i)
            {
                for (int j = 0; j < width / 2; ++j)
                {
                    std::swap(corners.at(i * width + j), corners.at(i * width + width - j - 1));
                }
            }
        }

        p0 = corners.at(0)->pt;
        p2 = corners.at(width)->pt;

        // check if we need to rotate the board
        if (p2.y < p0.y)
        {
            std::vector<ChessboardCornerPtr> outputCorners;
            outputCorners.resize(corners.size());

            for (int i = 0; i < height; ++i)
            {
                for (int j = 0; j < width; ++j)
                {
                    outputCorners.at(i * width + j) = corners.at((height - i - 1) * width + width - j - 1);
                }
            }

            corners = outputCorners;
        }

        return true;
    }

    void
    Chessboard::getQuadrangleHypotheses(const std::vector<std::vector<cv::Point>> &contours,
                                        std::vector<std::pair<float, int>> &quads,
                                        int classId) const
    {
        const float minAspectRatio = 0.2f;
        const float maxAspectRatio = 5.0f;
        const float minBoxSize = 10.0f;

        for (size_t i = 0; i < contours.size(); ++i)
        {
            cv::RotatedRect box = cv::minAreaRect(contours.at(i));
            float boxSize = std::max(box.size.width, box.size.height);
            if (boxSize < minBoxSize)
            {
                continue;
            }

            float aspectRatio = box.size.width / std::max(box.size.height, 1.0f);

            if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio)
            {
                continue;
            }

            quads.push_back(std::pair<float, int>(boxSize, classId));
        }
    }

    bool less_pred(const std::pair<float, int> &p1, const std::pair<float, int> &p2)
    {
        return p1.first < p2.first;
    }

    void countClasses(const std::vector<std::pair<float, int>> &pairs, size_t idx1, size_t idx2, std::vector<int> &counts)
    {
        counts.assign(2, 0);
        for (size_t i = idx1; i != idx2; ++i)
        {
            counts[pairs[i].second]++;
        }
    }

    bool
    Chessboard::checkChessboard(const cv::Mat &image, cv::Size patternSize) const
    {
        const int erosionCount = 1;
        const float blackLevel = 20.f;
        const float whiteLevel = 130.f;
        const float blackWhiteGap = 70.f;

        cv::Mat white = image.clone();

        cv::Mat black = image.clone();

        cv::erode(white, white, cv::Mat(), cv::Point(-1, -1), erosionCount);
        cv::dilate(black, black, cv::Mat(), cv::Point(-1, -1), erosionCount);

        cv::Mat thresh(image.rows, image.cols, CV_8UC1);

        bool result = false;
        for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f)
        {
            cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, CV_THRESH_BINARY);

            std::vector<std::vector<cv::Point>> contours;
            std::vector<cv::Vec4i> hierarchy;

            // Initialize contour retrieving routine
            cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

            std::vector<std::pair<float, int>> quads;
            getQuadrangleHypotheses(contours, quads, 1);

            cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV);

            cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
            getQuadrangleHypotheses(contours, quads, 0);

            const size_t min_quads_count = patternSize.width * patternSize.height / 2;
            std::sort(quads.begin(), quads.end(), less_pred);

            // now check if there are many hypotheses with similar sizes
            // do this by floodfill-style algorithm
            const float sizeRelDev = 0.4f;

            for (size_t i = 0; i < quads.size(); ++i)
            {
                size_t j = i + 1;
                for (; j < quads.size(); ++j)
                {
                    if (quads[j].first / quads[i].first > 1.0f + sizeRelDev)
                    {
                        break;
                    }
                }

                if (j + 1 > min_quads_count + i)
                {
                    // check the number of black and white squares
                    std::vector<int> counts;
                    countClasses(quads, i, j, counts);
                    const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f));
                    const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * floorf(patternSize.height / 2.0f));
                    if (counts[0] < blackCount * 0.75f ||
                        counts[1] < whiteCount * 0.75f)
                    {
                        continue;
                    }
                    result = true;
                    break;
                }
            }
        }

        return result;
    }

    bool
    Chessboard::checkBoardMonotony(std::vector<ChessboardCornerPtr> &corners,
                                   cv::Size patternSize)
    {
        const float threshFactor = 0.2f;

        Spline splineXY, splineYX;
        splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC);
        splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC);
        splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC);
        splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC);

        // check if each row of corners approximates a cubic spline
        for (int i = 0; i < patternSize.height; ++i)
        {
            splineXY.clear();
            splineYX.clear();

            cv::Point2f p[3];
            p[0] = corners.at(i * patternSize.width)->pt;
            p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt;
            p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt;

            for (int j = 0; j < 3; ++j)
            {
                splineXY.addPoint(p[j].x, p[j].y);
                splineYX.addPoint(p[j].y, p[j].x);
            }

            for (int j = 1; j < patternSize.width - 1; ++j)
            {
                cv::Point2f &p_j = corners.at(i * patternSize.width + j)->pt;

                float thresh = std::numeric_limits<float>::max();

                // up-neighbor
                if (i > 0)
                {
                    cv::Point2f &neighbor = corners.at((i - 1) * patternSize.width + j)->pt;
                    thresh = fminf(thresh, cv::norm(neighbor - p_j));
                }
                // down-neighbor
                if (i < patternSize.height - 1)
                {
                    cv::Point2f &neighbor = corners.at((i + 1) * patternSize.width + j)->pt;
                    thresh = fminf(thresh, cv::norm(neighbor - p_j));
                }
                // left-neighbor
                {
                    cv::Point2f &neighbor = corners.at(i * patternSize.width + j - 1)->pt;
                    thresh = fminf(thresh, cv::norm(neighbor - p_j));
                }
                // right-neighbor
                {
                    cv::Point2f &neighbor = corners.at(i * patternSize.width + j + 1)->pt;
                    thresh = fminf(thresh, cv::norm(neighbor - p_j));
                }

                thresh *= threshFactor;

                if (fminf(fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > thresh)
                {
                    return false;
                }
            }
        }

        // check if each column of corners approximates a cubic spline
        for (int j = 0; j < patternSize.width; ++j)
        {
            splineXY.clear();
            splineYX.clear();

            cv::Point2f p[3];
            p[0] = corners.at(j)->pt;
            p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt;
            p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt;

            for (int i = 0; i < 3; ++i)
            {
                splineXY.addPoint(p[i].x, p[i].y);
                splineYX.addPoint(p[i].y, p[i].x);
            }

            for (int i = 1; i < patternSize.height - 1; ++i)
            {
                cv::Point2f &p_i = corners.at(i * patternSize.width + j)->pt;

                float thresh = std::numeric_limits<float>::max();

                // up-neighbor
                {
                    cv::Point2f &neighbor = corners.at((i - 1) * patternSize.width + j)->pt;
                    thresh = fminf(thresh, cv::norm(neighbor - p_i));
                }
                // down-neighbor
                {
                    cv::Point2f &neighbor = corners.at((i + 1) * patternSize.width + j)->pt;
                    thresh = fminf(thresh, cv::norm(neighbor - p_i));
                }
                // left-neighbor
                if (j > 0)
                {
                    cv::Point2f &neighbor = corners.at(i * patternSize.width + j - 1)->pt;
                    thresh = fminf(thresh, cv::norm(neighbor - p_i));
                }
                // right-neighbor
                if (j < patternSize.width - 1)
                {
                    cv::Point2f &neighbor = corners.at(i * patternSize.width + j + 1)->pt;
                    thresh = fminf(thresh, cv::norm(neighbor - p_i));
                }

                thresh *= threshFactor;

                if (fminf(fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > thresh)
                {
                    return false;
                }
            }
        }

        return true;
    }

    bool
    Chessboard::matchCorners(ChessboardQuadPtr &quad1, int corner1,
                             ChessboardQuadPtr &quad2, int corner2) const
    {
        // First Check everything from the viewpoint of the
        // current quad compute midpoints of "parallel" quad
        // sides 1
        float x1 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1 + 1) % 4]->pt.x) / 2;
        float y1 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1 + 1) % 4]->pt.y) / 2;
        float x2 = (quad1->corners[(corner1 + 2) % 4]->pt.x + quad1->corners[(corner1 + 3) % 4]->pt.x) / 2;
        float y2 = (quad1->corners[(corner1 + 2) % 4]->pt.y + quad1->corners[(corner1 + 3) % 4]->pt.y) / 2;
        // compute midpoints of "parallel" quad sides 2
        float x3 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1 + 3) % 4]->pt.x) / 2;
        float y3 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1 + 3) % 4]->pt.y) / 2;
        float x4 = (quad1->corners[(corner1 + 1) % 4]->pt.x + quad1->corners[(corner1 + 2) % 4]->pt.x) / 2;
        float y4 = (quad1->corners[(corner1 + 1) % 4]->pt.y + quad1->corners[(corner1 + 2) % 4]->pt.y) / 2;

        // MARTIN: Heuristic
        // For corner2 of quad2 to be considered,
        // it needs to be on the same side of the two lines as
        // corner1. This is given, if the cross product has
        // the same sign for both computations below:
        float a1 = x1 - x2;
        float b1 = y1 - y2;
        // the current corner
        float c11 = quad1->corners[corner1]->pt.x - x2;
        float d11 = quad1->corners[corner1]->pt.y - y2;
        // the candidate corner
        float c12 = quad2->corners[corner2]->pt.x - x2;
        float d12 = quad2->corners[corner2]->pt.y - y2;
        float sign11 = a1 * d11 - c11 * b1;
        float sign12 = a1 * d12 - c12 * b1;

        float a2 = x3 - x4;
        float b2 = y3 - y4;
        // the current corner
        float c21 = quad1->corners[corner1]->pt.x - x4;
        float d21 = quad1->corners[corner1]->pt.y - y4;
        // the candidate corner
        float c22 = quad2->corners[corner2]->pt.x - x4;
        float d22 = quad2->corners[corner2]->pt.y - y4;
        float sign21 = a2 * d21 - c21 * b2;
        float sign22 = a2 * d22 - c22 * b2;

        // Also make shure that two border quads of the same row or
        // column don't link. Check from the current corner's view,
        // whether the corner diagonal from the candidate corner
        // is also on the same side of the two lines as the current
        // corner and the candidate corner.
        float c13 = quad2->corners[(corner2 + 2) % 4]->pt.x - x2;
        float d13 = quad2->corners[(corner2 + 2) % 4]->pt.y - y2;
        float c23 = quad2->corners[(corner2 + 2) % 4]->pt.x - x4;
        float d23 = quad2->corners[(corner2 + 2) % 4]->pt.y - y4;
        float sign13 = a1 * d13 - c13 * b1;
        float sign23 = a2 * d23 - c23 * b2;

        // Second: Then check everything from the viewpoint of
        // the candidate quad. Compute midpoints of "parallel"
        // quad sides 1
        float u1 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2 + 1) % 4]->pt.x) / 2;
        float v1 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2 + 1) % 4]->pt.y) / 2;
        float u2 = (quad2->corners[(corner2 + 2) % 4]->pt.x + quad2->corners[(corner2 + 3) % 4]->pt.x) / 2;
        float v2 = (quad2->corners[(corner2 + 2) % 4]->pt.y + quad2->corners[(corner2 + 3) % 4]->pt.y) / 2;
        // compute midpoints of "parallel" quad sides 2
        float u3 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2 + 3) % 4]->pt.x) / 2;
        float v3 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2 + 3) % 4]->pt.y) / 2;
        float u4 = (quad2->corners[(corner2 + 1) % 4]->pt.x + quad2->corners[(corner2 + 2) % 4]->pt.x) / 2;
        float v4 = (quad2->corners[(corner2 + 1) % 4]->pt.y + quad2->corners[(corner2 + 2) % 4]->pt.y) / 2;

        // MARTIN: Heuristic
        // For corner2 of quad2 to be considered,
        // it needs to be on the same side of the two lines as
        // corner1. This is given, if the cross product has
        // the same sign for both computations below:
        float a3 = u1 - u2;
        float b3 = v1 - v2;
        // the current corner
        float c31 = quad1->corners[corner1]->pt.x - u2;
        float d31 = quad1->corners[corner1]->pt.y - v2;
        // the candidate corner
        float c32 = quad2->corners[corner2]->pt.x - u2;
        float d32 = quad2->corners[corner2]->pt.y - v2;
        float sign31 = a3 * d31 - c31 * b3;
        float sign32 = a3 * d32 - c32 * b3;

        float a4 = u3 - u4;
        float b4 = v3 - v4;
        // the current corner
        float c41 = quad1->corners[corner1]->pt.x - u4;
        float d41 = quad1->corners[corner1]->pt.y - v4;
        // the candidate corner
        float c42 = quad2->corners[corner2]->pt.x - u4;
        float d42 = quad2->corners[corner2]->pt.y - v4;
        float sign41 = a4 * d41 - c41 * b4;
        float sign42 = a4 * d42 - c42 * b4;

        // Also make sure that two border quads of the same row or
        // column don't link. Check from the candidate corner's view,
        // whether the corner diagonal from the current corner
        // is also on the same side of the two lines as the current
        // corner and the candidate corner.
        float c33 = quad1->corners[(corner1 + 2) % 4]->pt.x - u2;
        float d33 = quad1->corners[(corner1 + 2) % 4]->pt.y - v2;
        float c43 = quad1->corners[(corner1 + 2) % 4]->pt.x - u4;
        float d43 = quad1->corners[(corner1 + 2) % 4]->pt.y - v4;
        float sign33 = a3 * d33 - c33 * b3;
        float sign43 = a4 * d43 - c43 * b4;

        // This time we also need to make shure, that no quad
        // is linked to a quad of another dilation run which
        // may lie INSIDE it!!!
        // Third: Therefore check everything from the viewpoint
        // of the current quad compute midpoints of "parallel"
        // quad sides 1
        float x5 = quad1->corners[corner1]->pt.x;
        float y5 = quad1->corners[corner1]->pt.y;
        float x6 = quad1->corners[(corner1 + 1) % 4]->pt.x;
        float y6 = quad1->corners[(corner1 + 1) % 4]->pt.y;
        // compute midpoints of "parallel" quad sides 2
        float x7 = x5;
        float y7 = y5;
        float x8 = quad1->corners[(corner1 + 3) % 4]->pt.x;
        float y8 = quad1->corners[(corner1 + 3) % 4]->pt.y;

        // MARTIN: Heuristic
        // For corner2 of quad2 to be considered,
        // it needs to be on the other side of the two lines than
        // corner1. This is given, if the cross product has
        // a different sign for both computations below:
        float a5 = x6 - x5;
        float b5 = y6 - y5;
        // the current corner
        float c51 = quad1->corners[(corner1 + 2) % 4]->pt.x - x5;
        float d51 = quad1->corners[(corner1 + 2) % 4]->pt.y - y5;
        // the candidate corner
        float c52 = quad2->corners[corner2]->pt.x - x5;
        float d52 = quad2->corners[corner2]->pt.y - y5;
        float sign51 = a5 * d51 - c51 * b5;
        float sign52 = a5 * d52 - c52 * b5;

        float a6 = x8 - x7;
        float b6 = y8 - y7;
        // the current corner
        float c61 = quad1->corners[(corner1 + 2) % 4]->pt.x - x7;
        float d61 = quad1->corners[(corner1 + 2) % 4]->pt.y - y7;
        // the candidate corner
        float c62 = quad2->corners[corner2]->pt.x - x7;
        float d62 = quad2->corners[corner2]->pt.y - y7;
        float sign61 = a6 * d61 - c61 * b6;
        float sign62 = a6 * d62 - c62 * b6;

        // Fourth: Then check everything from the viewpoint of
        // the candidate quad compute midpoints of "parallel"
        // quad sides 1
        float u5 = quad2->corners[corner2]->pt.x;
        float v5 = quad2->corners[corner2]->pt.y;
        float u6 = quad2->corners[(corner2 + 1) % 4]->pt.x;
        float v6 = quad2->corners[(corner2 + 1) % 4]->pt.y;
        // compute midpoints of "parallel" quad sides 2
        float u7 = u5;
        float v7 = v5;
        float u8 = quad2->corners[(corner2 + 3) % 4]->pt.x;
        float v8 = quad2->corners[(corner2 + 3) % 4]->pt.y;

        // MARTIN: Heuristic
        // For corner2 of quad2 to be considered,
        // it needs to be on the other side of the two lines than
        // corner1. This is given, if the cross product has
        // a different sign for both computations below:
        float a7 = u6 - u5;
        float b7 = v6 - v5;
        // the current corner
        float c71 = quad1->corners[corner1]->pt.x - u5;
        float d71 = quad1->corners[corner1]->pt.y - v5;
        // the candidate corner
        float c72 = quad2->corners[(corner2 + 2) % 4]->pt.x - u5;
        float d72 = quad2->corners[(corner2 + 2) % 4]->pt.y - v5;
        float sign71 = a7 * d71 - c71 * b7;
        float sign72 = a7 * d72 - c72 * b7;

        float a8 = u8 - u7;
        float b8 = v8 - v7;
        // the current corner
        float c81 = quad1->corners[corner1]->pt.x - u7;
        float d81 = quad1->corners[corner1]->pt.y - v7;
        // the candidate corner
        float c82 = quad2->corners[(corner2 + 2) % 4]->pt.x - u7;
        float d82 = quad2->corners[(corner2 + 2) % 4]->pt.y - v7;
        float sign81 = a8 * d81 - c81 * b8;
        float sign82 = a8 * d82 - c82 * b8;

        // Check whether conditions are fulfilled
        if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0)) &&
            ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0)) &&
            ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0)) &&
            ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0)) &&
            ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0)) &&
            ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0)) &&
            ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0)) &&
            ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0)) &&
            ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0)) &&
            ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0)) &&
            ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0)) &&
            ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0)))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

}
